/**
@author zhongyang
该模块是用来估计激光雷达实际测得的无人机的真实高度
假设激光雷达测距面是一个平整的面
订阅信息是，通过imu获得的姿态信息 attitude
                激光雷达测的距离信息 h_distance
发布数据是真实的高度 h_real
*/
#include <ros/ros.h>
#include <geometry_msgs/Vector3.h>
#include <math.h>
#include <std_msgs/Float64.h>

using namespace std;

geometry_msgs::Vector3 att;
std_msgs::Float64 h_distance;

//订阅姿态角信息
void att_sub_callback(geometry_msgs::Vector3::ConstPtr msg)
{
    att.x = msg->x;
    att.y = msg->y;
    att.z = msg->z;
}

//订阅距离值信息
void h_distance_sub_callback(std_msgs::Float64::ConstPtr msg)
{
    h_distance.data = msg->data;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lidarAltitude");
    ros::NodeHandle nh;

    ros::Subscriber att_sub = nh.subscribe("attitude", 50, att_sub_callback);
    ros::Subscriber h_distance_sub = nh.subscribe("h_distance", 50, h_distance_sub_callback);

    ros::Publisher lidarAltitude_pub = nh.advertise<std_msgs::Float64>("lidarAltitude", 50);

    ros::Rate loop_rate(50);
    std_msgs::Float64 lidarAltitude;

    while(ros::ok())
    {
        lidarAltitude.data = h_distance.data * cos(att.x) * cos(att.y);

        cout<<"real h is: "<<lidarAltitude.data<<endl;

        //发布测得高度
        lidarAltitude_pub.publish(lidarAltitude);

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}
